#include <config.h>
#include <linux/linkage.h>
#include <asm/regdef.h>
#include <asm/addrspace.h>
#include <asm/loongarch.h>
#include <mach/loongson.h>

#include "ns16550.h"

#define RESERVED_COREMASK	0xfff0
#define BOOTCORE_ID		    0
#define SHUTDOWN_MASK	    0

#define FN_OFF			    0x020
#define SP_OFF			    0x028
#define GP_OFF			    0x030
#define A1_OFF			    0x038

#ifndef CONFIG_SYS_INIT_SP_ADDR
#define CONFIG_SYS_INIT_SP_ADDR		PHYS_TO_CACHED(0x82000000)	// 32MB
#endif

// Note: the Address mapping window NOT setup yet.
// 		 so, use the real physicl address.
.macro watchdog_open
	//enable watch DOG.
	li.d	t1, 0x1fe27000
	li.w	t2, 0x2fffffff
	st.w	t2, t1, 0x38

	ld.w	t2, t1, 0x30
	li.w	t3, 0x2
	or	    t2, t2, t3
	st.w	t2, t1, 0x30	//enable watchdog

	li.w	t2, 0x1
	st.w	t2, t1, 0x34	//set watchdog time
.endm

.macro watchdog_close
	//disable watch DOG.
	li.d	t1, 0x1fe27000
	ld.w	t2, t1, 0x30
	li.w	t3, ~0x2
	and	    t2, t2, t3
	st.w	t2, t1, 0x30
.endm

ENTRY(lowlevel_init)
    or     a7, ra, zero

	/* slave core run to slave_main */
	csrrd   t0, 0x20
	andi    t0, t0, 0x3ff
	li.d    a0, CACHED_MEMORY_ADDR
	andi    t1, t0, 0x3             /* core id */
	slli.d  t2, t1, 18
	or      a0, t2, a0              /* 256KB offset for the each core */
	andi    t2, t0, 0xc             /* node id */
	slli.d  t2, t2, 42
	or      a0, t2, a0              /* get the L2 cache address */

	slli.d  t1, t1, 8
	or      t1, t2, t1

	li.d    t2, 0x1fe01000
	or      t1, t2, t1

	li.d    t3, RESERVED_COREMASK
	andi    t3, t3, 0xf
	li.d    t1, 0x1
	sll.w   t1, t1, t0
	and     t3, t1, t3
	bnez    t3, wait_to_be_killed

	li.d    t2, BOOTCORE_ID
	bne     t0, t2, slave_main

	li.d    a1, 0x1fe004d4
	ld.w    t2, a1, 0
	xori    t2, t2, SHUTDOWN_MASK
	st.w    t2, a1, 0

	b       1f

wait_to_be_killed:
	b	    wait_to_be_killed
	nop
1:

    /* config bar for APB, set APB base addr */
    li.d	t0, 0xfe00001000
	li.d	t1, PHYS_TO_UNCACHED(0x1fe20000)
	st.w	t1,	t0,	0x10 	
	ld.w	t2,	t0,	0x04
	ori	    t2,	t2,	0x2
	st.w	t2,	t0,	0x04

	watchdog_close

	
	/* ACPI Power Button Status clear */
	li.d	t0, 0x1fe27000
	ld.w	t1, t0, 0xc
	ori	t1, t1, 0x100
	st.w	t1, t0, 0xc


	//pcie signal test copy
	li.d	t0, 0x1fe00000

	li.w	t1, 0xc2492331
	st.w	t1, t0, 0x580
	st.w	t1, t0, 0x5a0

	li.w	t1, 0xff3ff0a8
	st.w	t1, t0, 0x584
	st.w	t1, t0, 0x5a4

	li.w	t1, 0x27fff
	st.w	t1, t0, 0x588
	st.w	t1, t0, 0x5a8


/* cfg pcie copy*/
	li.d	a0, 0x4fff1002
	bl	ls2k_pcie_phy_write
	li.d	a0, 0x4fff1102
	bl	ls2k_pcie_phy_write
	li.d	a0, 0x4fff1202
	bl	ls2k_pcie_phy_write
	li.d	a0, 0x4fff1302
	bl	ls2k_pcie_phy_write

	li.d	t0, 0x1fe00430
	ld.w	t1, t0, 0
	li.w	t2, 0x30000
	or	t1, t1, t2	//pcie enable
	st.w	t1, t0, 0

/* pcie 0 port 0 */
	li.d	a0, 9
	bl	ls2k_pcie0_port_conf
/* pcie 0 port 1 */
	li.d	a0, 10
	bl	ls2k_pcie0_port_conf
/* pcie 0 port 2 */
	li.d	a0, 11
	bl	ls2k_pcie0_port_conf
/* pcie 0 port 3 */
	li.d	a0, 12
	bl	ls2k_pcie0_port_conf
/* pcie 1 port 0 */
	li.d	a0, 13
	bl	ls2k_pcie1_port_conf
/* pcie 1 port 1 */
	li.d	a0, 14
	bl	ls2k_pcie1_port_conf

	//初始化调试串口，2k没配置前的启动频率默认100MHz
	li.d  a0, (OSC_CLK/16)/CONFIG_BAUDRATE
	bl    ls2k1000_uart_init
	nop
//	PRINTSTR("\r\nserial init ok\r\n")

#include "ls2k1000_clk_config.S"
	
	/* Config SATA PHY: cpu复位后sata控制器默认使用外部晶振，
	但某些板卡使用内部晶振，需要修改寄存器切换到内部晶振模式，如果切换时间比较迟，
	如uboot启动后，这会导致驱动探测sata盘时不稳定，出现初始化失败的问题，
	所以这里把sata phy的初始化代码提前到汇编中进行，尽早切换晶振模式，减少不稳定。
	另外，建议sata使用外部晶振 */
	li.d   t1, 0x1fe00450
	li.w   t0, 0x30c31cf9
	st.w   t0, t1, 0x4
	li.w   t0, 0xf300040f
	st.w   t0, t1, 0x0

	li.w   t0, 0xb0c31cf9
	st.w   t0, t1, 0x4
	nop

#ifdef LOONGSON_SATA_USED_INTERNRL_CLK
	ld.w   a0, t1, 0x0
	li.w   a1, 0x2
	nor    a1, a1, zero
	and    a0, a0, a1
	st.d   a0, t1, 0x0
	nop
#endif
	ld.w   a0, t1, 0x0
	li.w   a1, 0x4
	or     a0, a0, a1
	st.d   a0, t1, 0x0
	nop

	ld.w   a0, t1, 0x0
	li.w   a1, 0x8
	or     a0, a0, a1
	st.d   a0, t1, 0x0
	nop

	li.w   t0, 0x30c31cf9
	st.w   t0, t1, 0x4
	nop

	/* Config SATA TX signal*/
	li.d   t0, 0x1403f1002
	st.d   t0, t1, 0x8


 	// Fix the Gmac0  multi-func to enable Gmac1
	li.d	t0, 0x1fe03800
	li.d	a0, 0xffffff0000ffffff
	st.d	a0, t0, 0x08

	li.d	t0, 0xfe00001800
	li.w	a0, 0x0080ff08
	st.w	a0, t0, 0x0c

	// Set the invalid BAR to read only
	li.d	t0, 0x1fe03800
	li.d	a0, 0xff00ff0000fffff0
	st.d	a0, t0, 0x00
	st.d	a0, t0, 0x08
	st.d	a0, t0, 0x10
	st.d	a0, t0, 0x18
	st.d	a0, t0, 0x20
	st.d	a0, t0, 0x28
	st.d	a0, t0, 0x30
	st.d	a0, t0, 0x38
	st.d	a0, t0, 0x40
	st.d	a0, t0, 0x48
	st.d	a0, t0, 0x50

	li.d	t0, 0x1fe00430
	ld.w 	a2, t0, 0
	// enable pcie0 and pcie1, dvo0 and dvo1 pin output
	li.w	t1, 0x30012
	or	a2, a2, t1
   	st.w    a2, t0, 0


	li.d	t0, 0x1fe00420
	ld.w	a2, t0, 0
	//enable sdio, pwm0, pwm1, i2c0, i2c1, nand, sata, gmac1
	//no hda, no ac97, no i2s
	li.w	t1, 0x103f08
	or	a2, a2, t1
	st.w	a2, t0, 0

	/* spi speedup */
	li.d	t0, 0x1fff0220
	li.w	t1, 0x47
	st.b	t1, t0, 0x4


    or      ra, a7, zero
    jirl	zero, ra, 0
ENDPROC(lowlevel_init)


/*
 * a0 [0:15] phy configure address, [16:31] phy configure data
 */
ENTRY(ls2k_pcie_phy_write)
	li.d	a1, 0x100000000
	or		a0, a1, a0
	li.d	a1, 0x1fe00590
	st.d	a0, a1, 0x0
	st.d	a0, a1, 0x20
1:
	ld.w	a0, a1, 0x4
	andi	a0, a0, (1 << 2)
	beqz	a0, 1b
	jirl	zero, ra, 0
ENDPROC(ls2k_pcie_phy_write)

/*
 * a0 is device number
 */
ENTRY(ls2k_pcie0_port_conf)
	slli.w	a0, a0, 11
	li.d	a1, 0xfe0800000c
	or		a1, a1, a0

	li.w	a2, 0xfff9ffff
	ld.w	a3, a1, 0x0
	and		a3, a3, a2
	li.w	a2, 0x20000
	or		a3, a3, a2
	st.w	a3, a1, 0x0

	li.d	a1, 0xfe0700001c
	or		a1, a1, a0
	ld.w	a3, a1, 0x0
	li.w	a2, (0x1 << 26)
	or		a3, a3, a2
	st.w	a3, a1, 0x0

	li.d	a1, 0xfe00000000
	or		a1, a1, a0
	ld.w	a3, a1, 0x78
	li.w	a2, ~(0x7 << 12)
	and		a3, a3, a2
	li.w	a2, 0x1000
	or		a3, a3, a2
	st.w	a3, a1, 0x78
	li.w	a2, 0x11000000
	st.w	a2, a1, 0x10

	li.d	a1, 0x11000000

	li.w	a2, ~((0x7 << 18) | (0x7 << 2))
	ld.w	a3, a1, 0x54
	and		a3, a3, a2
	st.w	a3, a1, 0x54

	ld.w	a3, a1, 0x58
	and		a3, a3, a2
	st.w	a3, a1, 0x58

	li.d	a2, 0xff204f
	st.w	a2, a1, 0x0

	jirl	zero, ra, 0
ENDPROC(ls2k_pcie0_port_conf)


ENTRY(ls2k_pcie1_port_conf)
	slli.w	a0, a0, 11
	li.d	a1, 0xfe0800000c
	or		a1, a1, a0

	li.w	a2, 0xfff9ffff
	ld.w	a3, a1, 0x0
	and		a3, a3, a2
	li.w	a2, 0x20000
	or		a3, a3, a2
	st.w	a3, a1, 0x0

	li.d	a1, 0xfe0700001c
	or		a1, a1, a0
	ld.w	a3, a1, 0x0
	li.w	a2, (0x1 << 26)
	or		a3, a3, a2
	st.w	a3, a1, 0x0

	li.d	a1, 0xfe00000000
	or		a1, a1, a0
	ld.w	a3, a1, 0x78
	li.w	a2, ~(0x7 << 12)
	and		a3, a3, a2
	li.w	a2, 0x1000
	or		a3, a3, a2
	st.w	a3, a1, 0x78
	li.w	a2, 0x10000000
	st.w	a2, a1, 0x10

	li.d	a1, 0x10000000

	li.w	a2, ~((0x7 << 18) | (0x7 << 2))
	ld.w	a3, a1, 0x54
	and		a3, a3, a2
	st.w	a3, a1, 0x54

	ld.w	a3, a1, 0x58
	and		a3, a3, a2
	st.w	a3, a1, 0x58

	li.d	a2, 0xff204f
	st.w	a2, a1, 0x0

	jirl	zero, ra, 0
ENDPROC(ls2k_pcie1_port_conf)

// Note: the Address mapping window setting up already.
// 		 so, use the Mapped address 0x8xxx... or 0x9xxx...
ENTRY(init_serial)
	or     a4, ra, zero
	ori    a3, a0, 0

	nop

	or     ra, a4, zero
	jirl   zero, ra, 0
ENDPROC(init_serial)

ENTRY(ls2k1000_uart_init)
	or     a4, ra, zero
	ori    a3, a0, 0

	li.d   a0, UART_BASE_ADDR
	li.w   a1, FIFO_ENABLE|FIFO_RCV_RST|FIFO_XMT_RST|FIFO_TRIGGER_4
	st.b   a1, a0, NSREG(NS16550_FIFO)
	li.w   a1, CFCR_DLAB                  #DLAB
	st.b   a1, a0, NSREG(NS16550_CFCR)
	st.b   a3, a0, NSREG(NS16550_DATA)
	srli.d a3, a3, 8
	st.b   a3, a0, NSREG(NS16550_IER)     #set BRDH
	li.w   a1, CFCR_8BITS                 #8bit
	st.b   a1, a0, NSREG(NS16550_CFCR)
	li.w   a1, MCR_DTR|MCR_RTS
	st.b   a1, a0, NSREG(NS16550_MCR)
	li.w   a1, 0x0
	st.b   a1, a0, NSREG(NS16550_IER)

	or      ra, a4, zero
	jirl	zero, ra, 0
ENDPROC(ls2k1000_uart_init)

ENTRY(ram_init)
	or	s1, ra, zero

#if !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD)
/*	PRINTSTR("\r\nlock scache for early stack: ")
	li.d	a0, LOCK_CACHE_BASE
	bl		hexserial64
	PRINTSTR(" - ")
	li.d	a0, LOCK_CACHE_BASE + LOCK_CACHE_SIZE
	bl		hexserial64*/

	li.d	t0, PHYS_TO_UNCACHED(0x1fe00200)
	li.d	t1, ~(LOCK_CACHE_SIZE - 1)
	st.d	t1, t0, 0x40
	li.d	t1, (LOCK_CACHE_BASE & 0xffffffffffff) | (1 << 63)
	st.d	t1, t0, 0x0
//	PRINTSTR("\r\nLock Scache Done.\r\n")
#endif

#if defined(CONFIG_SPL_BUILD)
	// copy spl code to locked scache
	li.d	t0, PHYS_TO_UNCACHED(BOOT_SPACE_BASE)
	la		t1, __text_start
	la		t2, __image_copy_end
1:
	ld.w	t3, t0, 0
	st.w	t3, t1, 0
	addi.d	t0, t0, 4
	addi.d	t1, t1, 4
	blt		t1, t2, 1b

	// clear bss
	la		t0, __bss_start
	la		t1, __bss_end
2:
	st.w	zero, t0, 0
	addi.d	t0, t0, 4
	blt		t0, t1, 2b

	// jump to cache
	la		t0, jump_cache
	jirl	zero, t0, 0
jump_cache:

	li.d	a0, LOCK_CACHE_BASE + LOCK_CACHE_SIZE

#elif !defined(CONFIG_SPL)
	li.d	t0, LOCK_CACHE_BASE + LOCK_CACHE_SIZE
	or		sp, t0, zero

	// PRINTSTR("jump to ddr_init\r\n");
	la		t8, ddr_init
	jirl	ra, t8, 0

	// PRINTSTR("unlock scache\r\n")
	// unlock scache
	li.d	t0, PHYS_TO_UNCACHED(0x1fe00200)
	st.d	zero, t0, 0x40
	st.d	zero, t0, 0
	li.d	t0, LOCK_CACHE_BASE
	li.d	t1, LOCK_CACHE_BASE + LOCK_CACHE_SIZE
2:
	cacop	0x13, t0, 0
	addi.d	t0, t0, 0x40
	blt		t0, t1, 2b

	// return the sp addr.
	li.d	a0, CONFIG_SYS_INIT_SP_ADDR
#else
	li.d	a0, CONFIG_SYS_INIT_SP_ADDR
#endif

	or		ra, s1, zero
	jirl	zero, ra, 0
ENDPROC(ram_init)


/******************************************************
 *used: a0 - char to print, a1, a2
 ******************************************************/
ENTRY(printchar) 
	li.d	a1, UART_BASE_ADDR
1:
	ld.bu	a2, a1, 0x5
	andi	a2, a2, 0x20
	beqz	a2, 1b

	st.b	a0, a1, 0

	jirl	zero, ra, 0
ENDPROC(printchar)

ENTRY(update_slave_core)
	li.d	t2, PHYS_TO_UNCACHED(0x1fe01100)

	la	t0, slave_main_call
	st.d	t0, t2, FN_OFF

	// wait a while
	li.d	t0, 20000000
1:
	addi.d	t0, t0, -1
	bnez	t0, 1b

	jirl	zero, ra, 0
ENDPROC(update_slave_core)

ENTRY(clear_mailbox)
	csrrd   t0, 0x20

	andi    t0, t0, 0x3ff
	andi    t1, t0, 0x3
	slli.d  t1, t1, 0x8
	andi    t2, t0, 0xc
	slli.d  t2, t2, 42
	or      t1, t2, t1
	li.d    t2, PHYS_TO_UNCACHED(0x1fe01000)
	or      t1, t1, t2
	st.d    zero, t1, FN_OFF
	st.d    zero, t1, SP_OFF
	st.d    zero, t1, GP_OFF
	st.d    zero, t1, A1_OFF

	jirl zero, ra, 0x0
ENDPROC(clear_mailbox)

slave_main:
	/* a0, ap own cpu number
	 * t2, bsp mail box address
	 */
	bl      clear_mailbox

	/* t1 store mailbox base so don't changing it */

slave_main_call:
	st.d    zero, t1, FN_OFF

waitforinit:
	li.d      t0, 0x100

idle1000:
	addi.w  t0, t0, -1
	bnez    t0, idle1000
	/*csr finally filled the low 32 bits*/
	ld.d    t0, t1, FN_OFF
	beqz    t0, waitforinit

	ld.d    t0, t1, FN_OFF

	or      ra, t0,zero

	li.d     t3, CACHED_MEMORY_ADDR

	ld.d    t0, t1, SP_OFF
	or      t0, t0, t3
	or      sp, t0, zero

	ld.d    t0, t1, GP_OFF
	or      t0, t0, t3
	or      gp, t0, zero

	# slave core jump to kernel, byebye
	jirl    zero, ra, 0x0
//end slave_main
